
#include "det_box_DefaultDetector.h"

bool CCandidateDetection::CompareBy_Bigger_Confidence(const CCandidateDetection& a, const CCandidateDetection& b)
{
	return a.Confidence > b.Confidence;
}

bool CCandidateDetection::CompareBy_hwyx(const CCandidateDetection& a, const CCandidateDetection& b)
{
	return a.ipp < b.ipp;
}

bool CCandidateDetection::CompareBy_Tag(const CCandidateDetection& a, const CCandidateDetection& b)
{
	return a.Tag < b.Tag;
}

CCandidateDetection::CCandidateDetection()
{
	ipp.InitNew(0.0, 0.0, 1.0, 1.0);
	Confidence = 0.0;
	Tag = 0;
}

void CCandidateDetection::Build(CImagePyramidPositionFeatureExtractor* pIPPFE, CBinaryClassifier* pClassifier, int Tag)
{
	int tPrediction;

	if (pIPPFE==NULL || pClassifier==NULL)
		this->Confidence = 0.0;
	else
	{
		pIPPFE->SetQuery(ipp);
		pClassifier->Classify(pIPPFE, this->Confidence, tPrediction);
	}
}

void CCandidateDetection::Build(const CImagePyramidPosition& ipp, CImagePyramidPositionFeatureExtractor* pIPPFE, CBinaryClassifier* pClassifier, int Tag)
{
	this->ipp = ipp;
	Build(pIPPFE, pClassifier, Tag);
}

bool CCandidateDetection::InputFromStream(istream& inStream)
{
	if (inStream>>Confidence>>Tag)
		return ipp.InputFromStream(inStream);
	else
		return false;
}

void CCandidateDetection::OutputToStream(ostream& outStream)
{
	outStream.precision(6);
	outStream<<setw(12)<<fixed<<Confidence;
	outStream<<"\t"<<setw(5)<<Tag;
	ipp.OutputToStream(outStream);
	outStream<<endl;
}

bool CCandidateDetection::InputListFromStream(istream& inStream, CSimpleTypeArray<CCandidateDetection>& dstList)
{
	size_t i;
	size_t s;
	CCandidateDetection* pSrc;
	char z[100];
	if (!(inStream>>z) || strcmp(z,"#")!=0) return false;
	if (!(inStream>>s)) return false;
	dstList.Resize(s);
	pSrc = dstList.pElement;
	for ( i = 0; i < s; ++i )
		if (!((pSrc++)->InputFromStream(inStream)))
		{
			dstList.Resize(0);
			return false;
		}
	return true;
}

bool CCandidateDetection::InputListFromFile(const char* strFN_CanDetList, CSimpleTypeArray<CCandidateDetection>& dstList)
{
	bool ret;
	ifstream inFile(strFN_CanDetList);
	ret = InputListFromStream(inFile, dstList);
	inFile.clear();
	inFile.close();
	return ret;
}

bool CCandidateDetection::InputListFromFile(const string& strFN_CanDetList, CSimpleTypeArray<CCandidateDetection>& dstList)
{
	return InputListFromFile(strFN_CanDetList.c_str(), dstList);
}

void CCandidateDetection::OutputListToStream(ostream& outStream, const CSimpleTypeArray<CCandidateDetection>& srcList)
{
	unsigned __int64 i;
	CCandidateDetection* pSrc;

	outStream<<"#"<<endl;
	outStream<<srcList.Size<<endl;
	pSrc = srcList.pElement;
	for ( i = 0; i < srcList.Size; ++i )
		(pSrc++)->OutputToStream(outStream);
}

void CCandidateDetection::OutputListToFile(const char* strFN_CanDetList, const CSimpleTypeArray<CCandidateDetection>& srcList)
{
	ofstream outFile(strFN_CanDetList);
	OutputListToStream(outFile, srcList);
	outFile.clear();
	outFile.close();
}

void CCandidateDetection::OutputListToFile(const string& strFN_CanDetList, const CSimpleTypeArray<CCandidateDetection>& srcList)
{
	OutputListToFile(strFN_CanDetList.c_str(), srcList);
}

void CSlidingWindowDetector::myInit(
	CImagePyramidPositionFeatureExtractor* pIPPFE,
	CBinaryClassifier* pClassifier,
	CNonMaximumSupperessor* pNMSor
	)
{
	this->pIPPFE = pIPPFE;
	this->pClassifier = pClassifier;
	this->pNMSor = pNMSor;
}

void CSlidingWindowDetector::SetImage(const CImg<int>& imgSourceRGB)
{
	pIPPFE->SetImage(imgSourceRGB, "");
	CandidateDetectionList.Clear();
	ReducedDetectionList.Clear();
}

void CSlidingWindowDetector::BuildCandidateDetectionList(const CImageWindowContext::CSlidingWindowList& swl)
{
	int i;
	CCandidateDetection tDet;

	CandidateDetectionList.Clear();
	for ( i = 0; i < swl.Size; ++i )
	{
		tDet.Build(swl.ippList.pElement[i], pIPPFE, pClassifier);
		CandidateDetectionList.PushBack(tDet);

		//cout<<i<<" / "<<swl.Size<<endl;
	}
	sort(CandidateDetectionList.Begin(), CandidateDetectionList.End(), &CCandidateDetection::CompareBy_Bigger_Confidence);
}

void CSlidingWindowDetector::DoNMS()
{
	pNMSor->DoNMS(ReducedDetectionList, CandidateDetectionList);
	sort(ReducedDetectionList.Begin(), ReducedDetectionList.End(), &CCandidateDetection::CompareBy_Bigger_Confidence);
}

int CSlidingWindowDetector::CheckDetections(
	CSimpleTypeArray<CCandidateDetection>& DetectionList,
	CSimpleTypeArray<CRectBox<int>>& GtBoxList,
	double BaseDetectionBoxW, double BaseDetectionBoxH,
	double AO_Threshold, bool fAllowMultiDetection
	)
{
	int ret;
	int i,j,j_max;
	double AO, AO_max;
	bool* pGTBvisit = new bool[GtBoxList.Size];
	CRectBox<int> CurrentBox;

	ret = (int)GtBoxList.Size;
	for ( j = 0; j < (int)GtBoxList.Size; ++j )
		pGTBvisit[j] = true;
	sort(DetectionList.Begin(), DetectionList.End(), &CCandidateDetection::CompareBy_Bigger_Confidence);
	for ( i = 0; i < (int)DetectionList.Size; ++i )
	{
		CCandidateDetection& CurrentDetection = DetectionList.pElement[i];
		CImageWindowContext::ComputeRectBox(CurrentBox, CurrentDetection.ipp, BaseDetectionBoxW, BaseDetectionBoxH);

		AO_max = -1.0; j_max = -1;
		for ( j = 0; j < (int)GtBoxList.Size; ++j )
		{
			AO = CRectBox<int>::CalcAreaOfOverlap(CurrentBox, GtBoxList.pElement[j]);
			if (j_max<0 || AO>AO_max)
			{
				j_max = j;
				AO_max = AO;
			}
		}
		if (AO_max > AO_Threshold)
			if (pGTBvisit[j_max])
			{
				CurrentDetection.Tag = +1;
				pGTBvisit[j_max] = false;
				--ret;
			}
			else
				CurrentDetection.Tag = (fAllowMultiDetection ? 1 : 0);
		else
			CurrentDetection.Tag = 0;
	}

	delete[] pGTBvisit;

	return ret;
}

double CSlidingWindowDetector::Classify(const CImagePyramidPosition& ipp)
{
	double ret;
	int tPrediction;

	pIPPFE->SetQuery(ipp);
	pClassifier->Classify(pIPPFE, ret, tPrediction);
	return ret;
}

//CMeanShiftNMS::CParameters

void CMeanShiftNMS::CParameters::SetConfidenceThreshold(double ConfidenceThreshold)
{
	this->ConfidenceThreshold_Before = ConfidenceThreshold;
	this->ConfidenceThreshold_After = ConfidenceThreshold;
	this->KernelConfig.center = ConfidenceThreshold;
}

//CMeanShiftNMS::CNode

double CMeanShiftNMS::CNode::GetDistance2(const CNode& a, const CNode& b)
{
	return (CMyMath::sqr(a.X-b.X) + CMyMath::sqr(a.Y-b.Y) + CMyMath::sqr(a.lnW-b.lnW) + CMyMath::sqr(a.lnH-b.lnH));
}

double CMeanShiftNMS::CKernelNode::Calc_log_TransformFunction(double Confidence, const CKernelConfig& KernelConfig)
{
	switch (KernelConfig.tfMode)
	{
	case TFMODE_LOG_HARD:
		return CMyMath::log_Power((Confidence>KernelConfig.center ? Confidence-KernelConfig.center : 0.0), KernelConfig.scale);
	case TFMODE_SOFT:
		return CMyMath::log_Product((1+exp(KernelConfig.scale*(Confidence-KernelConfig.center))), -CMyMath::my_log(KernelConfig.scale));
		break;
	case TFMODE_SIGMOID:
		return 0.0 - CMyMath::my_log(1+exp(KernelConfig.scale*(Confidence-KernelConfig.center)));
		break;
	case TFMODE_HARD:
	default:
		return CMyMath::my_log(Confidence>KernelConfig.center ? Confidence-KernelConfig.center : 0.0);
		break;
	}
}

void CMeanShiftNMS::CKernelNode::myInit(const CNode& Node, double Confidence, const CKernelConfig& KernelConfig)
{
	this->node = Node;

	this->log_transformed_weight = Calc_log_TransformFunction(Confidence, KernelConfig);
	this->log_transformed_weight = CMyMath::log_Product(this->log_transformed_weight, Node.lnW);
	this->log_transformed_weight = CMyMath::log_Product(this->log_transformed_weight, Node.lnH);

	this->inv_dx = exp(Node.lnW) * KernelConfig.dx;
	this->inv_dy = exp(Node.lnH) * KernelConfig.dy;
	this->inv_dw = log(KernelConfig.dw);
	this->inv_dh = log(KernelConfig.dh);

	this->inv_dx = 1.0 / CMyMath::sqr(this->inv_dx);
	this->inv_dy = 1.0 / CMyMath::sqr(this->inv_dy);
	this->inv_dw = 1.0 / CMyMath::sqr(this->inv_dw);
	this->inv_dh = 1.0 / CMyMath::sqr(this->inv_dh);

	this->log_coefficient = CMyMath::my_log(sqrt(this->inv_dx * this->inv_dy * this->inv_dw * this->inv_dh));
	this->log_coefficient = CMyMath::log_Product(this->log_coefficient, this->log_transformed_weight);
}

void CMeanShiftNMS::CKernelNode::myInit(double Confidence, const CKernelConfig& KernelConfig)
{
	myInit(this->node, Confidence, KernelConfig);
}

CMeanShiftNMS::CMeanShiftNMS()
{
	ClearKernelNodes();
}

void CMeanShiftNMS::ClearKernelNodes()
{
	KernelNodes.Clear();
}

void CMeanShiftNMS::myInit(
	const CParameters& Parameters,
	CImagePyramidPositionFeatureExtractor* pIPPFE,
	CBinaryClassifier* pClassifier
	)
{
	_Parameters = Parameters;
	__pIPPFE = pIPPFE;
	__pClassifier = pClassifier;
	ClearKernelNodes();
}


void CMeanShiftNMS::BuildNode(CNode& dstNode, const CImagePyramidPosition& src_ipp)
{
	dstNode.X = src_ipp.xOld;
	dstNode.Y = src_ipp.yOld;
	dstNode.lnW = -log(src_ipp.xScale);
	dstNode.lnH = -log(src_ipp.yScale);
}

void CMeanShiftNMS::BuildIPP(CImagePyramidPosition& dst_ipp, const CNode& srcNode)
{
	dst_ipp.InitNew(srcNode.X, srcNode.Y, exp(-srcNode.lnW), exp(-srcNode.lnH));
}

void CMeanShiftNMS::BuildKernelNode(CKernelNode& dstKernelNode, const CCandidateDetection& srcDetection)
{
	BuildNode(dstKernelNode.node, srcDetection.ipp);
	dstKernelNode.myInit(srcDetection.Confidence, _Parameters.KernelConfig);
}

void CMeanShiftNMS::Insert(const CCandidateDetection& srcDetection)
{
	CKernelNode NewKernelNode;

	if (srcDetection.Confidence < _Parameters.ConfidenceThreshold_Before) return;

	BuildKernelNode(NewKernelNode, srcDetection);

	if (NewKernelNode.log_transformed_weight < -MAXIMUM_DOUBLE_NUM_C) return;

	KernelNodes.PushBack(NewKernelNode);
}

void CMeanShiftNMS::Insert(const CSimpleTypeArray<CCandidateDetection>& srcDetectionList)
{
	size_t i;

	for ( i = 0; i < srcDetectionList.Size; ++i )
		Insert(srcDetectionList.pElement[i]);
}

double CMeanShiftNMS::Promote(CNode& dstNode, const CNode& srcNode)
{
	double ret = 0.0;
	int i;
	double sw,tw;
	double* wlist = new double[KernelNodes.Size];
	double* pw;
	CKernelNode* pkn;
	CNode rnode;
	double zx,zy,zw,zh;
	double ux,uy,uw,uh;
	double tt;

	if (KernelNodes.Size < 1)
	{
		dstNode = srcNode;
		return ret = 0.0;
	}

	sw = -MAXIMUM_DOUBLE_NUM_V;
	//sw = 0.0;
	pw = wlist; pkn = KernelNodes.pElement;
	for ( i = 0; i < (int)KernelNodes.Size; ++i )
	{
		tt = 0.0;
		tt += pkn->inv_dx * CMyMath::sqr(srcNode.X - pkn->node.X);
		tt += pkn->inv_dy * CMyMath::sqr(srcNode.Y - pkn->node.Y);
		tt += pkn->inv_dw * CMyMath::sqr(srcNode.lnW - pkn->node.lnW);
		tt += pkn->inv_dh * CMyMath::sqr(srcNode.lnH - pkn->node.lnH);

		tt = CMyMath::log_Product(pkn->log_coefficient, -0.5 * tt);
		sw = CMyMath::log_Sum(sw, tt);
		//tt = pkn->coefficient * exp(-0.5 * tt);
		//sw += tt;

		*(pw++) = tt;
		++pkn;
	}
	
	if (sw < -MAXIMUM_DOUBLE_NUM_C)
	//if (sw<EPS)
	{
		sw = KernelNodes.Size * 1.0;
		for ( i = 0; i < (int)KernelNodes.Size; ++i )
			wlist[i] = 1.0;
	}
	else
	{
		for ( i = 0; i < (int)KernelNodes.Size; ++i )
			wlist[i] = exp(CMyMath::log_Product(wlist[i], -sw));
		sw = 1.0;
	}

	zx = zy = zw = zh = 0.0;
	ux = uy = uw = uh = 0.0;
	pw = wlist; pkn = KernelNodes.pElement;
	for ( i = 0; i < (int)KernelNodes.Size; ++i )
	{
		tw = (*(pw++) /= sw);
		if (tw>EPS)
		{
			tt = tw * pkn->inv_dx; zx += tt; ux += tt * pkn->node.X;
			tt = tw * pkn->inv_dy; zy += tt; uy += tt * pkn->node.Y;
			tt = tw * pkn->inv_dw; zw += tt; uw += tt * pkn->node.lnW;
			tt = tw * pkn->inv_dh; zh += tt; uh += tt * pkn->node.lnH;
		}
		++pkn;
	}
	delete[] wlist;
	rnode.X = ux / zx;
	rnode.Y = uy / zy;
	rnode.lnW = uw / zw;
	rnode.lnH = uh / zh;
	ret = CNode::GetDistance2(rnode, srcNode);
	dstNode = rnode;
	return ret;
}

double CMeanShiftNMS::Promote(CNode& Node)
{
	return Promote(Node, Node);
}

void CMeanShiftNMS::DoNMS(
	CSimpleTypeArray<CCandidateDetection>& dstDetectionList,
	const CSimpleTypeArray<CCandidateDetection>& srcDetectionList
	)
{
	int i,j,o;
	CCandidateDetection tDet;
	CNode tNode;
	double ee = EPS;
	double dxx, dyy, dsx, dsy;

	ClearKernelNodes();
	Insert(srcDetectionList);
	dstDetectionList.Clear();
	for ( i = 0; i < (int)srcDetectionList.Size; ++i )
	{
		tDet = srcDetectionList.pElement[i];
		if (tDet.Confidence < _Parameters.ConfidenceThreshold_Before) continue;

		BuildNode(tNode, tDet.ipp);
		for ( o = 0; ; ++o )
			if (Promote(tNode) < ee)
				break;
		BuildIPP(tDet.ipp, tNode);
		tDet.Confidence = 0.0;

		tDet.Confidence = srcDetectionList.pElement[i].Confidence;
		tDet.Tag = srcDetectionList.pElement[i].Tag;
		bool f_merged = false;
		for ( j = 0; j < (int)dstDetectionList.Size; ++j )
		{
			CImagePyramidPosition j_ipp = dstDetectionList.pElement[j].ipp;
			dxx = CMyMath::sqr(tDet.ipp.xOld - j_ipp.xOld) * (tDet.ipp.xScale * j_ipp.xScale);
			dyy = CMyMath::sqr(tDet.ipp.yOld - j_ipp.yOld) * (tDet.ipp.yScale * j_ipp.yScale);
			dsx = exp(abs(tDet.ipp.xScale - j_ipp.xScale));
			dsy = exp(abs(tDet.ipp.yScale - j_ipp.yScale));
			if (1
				&& dxx<4
				&& dyy<4
				&& dsx<=1.01
				&& dsy<=1.01
				)
			{
				f_merged = true;
				if (dstDetectionList.pElement[j].Confidence < tDet.Confidence)
					dstDetectionList.pElement[j] = tDet;
				break;
			}
		}
		if (f_merged) continue;

		dstDetectionList.PushBack(tDet);
	}

	if (__pIPPFE && __pClassifier)
	{
		sort(dstDetectionList.Begin(), dstDetectionList.End(), &CCandidateDetection::CompareBy_hwyx);
		for ( i = 0, j = 0; j < (int)dstDetectionList.Size; ++j )
			dstDetectionList.pElement[j].Build(__pIPPFE, __pClassifier);
	}

	for ( i = 0, j = 0; j < (int)dstDetectionList.Size; ++j )
	{
		if (dstDetectionList.pElement[j].Confidence > _Parameters.ConfidenceThreshold_After)
			dstDetectionList.pElement[i++] = dstDetectionList.pElement[j];
	}
	dstDetectionList.Resize(i);
	sort(dstDetectionList.Begin(), dstDetectionList.End(), &CCandidateDetection::CompareBy_Bigger_Confidence);
}

